#!/usr/bin/env python
import rospy

from geometry_msgs.msg import Twist

import sys, select, termios, tty

msg = """
Control The Robot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max move and rot speed by 10%
w/x : increase/decrease only move speed by 10%
e/c : increase/decrease only rot speed by 10%
space key, k : force stop
anything else : stop smoothly

CTRL-C to quit
"""

moveBindings = {
        'i':(1,0),
        'j':(0,1),
        'l':(0,-1),
        ',':(-1,0),
           }
rotBindings = {
        'u':(1,0),
        'o':(-1,0),
        'm':(1,1),
        '.':(-1,-1)
           }


speedBindings={
        'q':(1.1,1.1),
        'z':(.9,.9),
        'w':(1.1,1),
        'x':(.9,1),
        'e':(1,1.1),
        'c':(1,.9),
          }

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

speed = .2
turn = 1

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    rospy.init_node('robot_teleop')
    pub = rospy.Publisher('pwm', Twist, queue_size=3)

    x = 0
    y = 0
    z = 0
    status = 0
    count = 0
    acc = 0.1
    target_speedx = 0
    target_speedy = 0
    target_turn = 0
    control_speedx = 0
    control_speedy = 0
    control_turn = 0
    try:
        print (msg)
        print (vels(speed,turn))
        while(1):
            key = getKey()
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                y = moveBindings[key][1]
                count = 0
            elif key in rotBindings.keys():
                x = rotBindings[key][1]
                y = rotBindings[key][1]
                z = rotBindings[key][0]
                count = 0
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn = turn * speedBindings[key][1]
                count = 0

                print (vels(speed,turn))
                if (status == 14):
                    print (msg)
                status = (status + 1) % 15
            elif key == ' ' or key == 'k' :
                x = 0
                y = 0
                z = 0
                control_speedx = 0
                control_speedy = 0
                control_turn = 0
            else:
                count = count + 1
                if count > 4:
                    x = 0
                    y = 0
                    z = 0
                if (key == '\x03'):
                    break

            target_speedx = speed * x
            target_speedy = speed * y
            target_turn = turn*z

            if target_speedx > control_speedx:
                control_speedx = min( target_speedx, control_speedx + 0.02 )
            elif target_speedx < control_speedx:
                control_speedx = max( target_speedx, control_speedx - 0.02 )
            else:
                control_speedx = target_speedx

            if target_speedy > control_speedy:
                control_speedy = min( target_speedy, control_speedy + 0.02 )
            elif target_speedy < control_speedy:
                control_speedy = max( target_speedy, control_speedy - 0.02 )
            else:
                control_speedy = target_speedy
            
            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.1 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.1 )
            else:
                control_turn = target_turn

            twist = Twist()
            twist.linear.x = control_speedx*35+1525
            twist.linear.y = control_speedy
            twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0
            twist.angular.z = control_turn*5+80
            pub.publish(twist)


    except:
        print (e)

    finally:
        twist = Twist()
        twist.linear.x = 1500; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 80
        pub.publish(twist)

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

